// Robotic Arm
//
// *******************************************************************
// !!!!!!!!!!!!!!!!!!!!!!!!!!!! IMPORTANT !!!!!!!!!!!!!!!!!!!!!!!!!!!!
//   When reprogramming, make sure the LCD screen is UNPLUGGED!!!
//   If it is not, it will screw it up!  :(
//   If you screw it up, then uncomment "setStartScreen()" in LCDsetup()
//     and reprogram with the LCD still UNPLUGGED!
//     Once it has been programmed, plug in the LCD screen, then press
//     the restart button on the micro.  Wait till the orange light turns
//     off on the micro.  Unplug the LCD screen, comment out the "setStartScreen()"
//     that you uncommented earlier.  Reflash the micro (still no LCD attached).
//     Once this is done, then you're finally good to go.  So remember,
//   When reprogramming, make sure the LCD screen is UNPLUGGED!!!
// *******************************************************************


#include <Servo.h>
#include "avr/delay.h"

#define baudRate 19200   // communication speed for LCD display

#define intPin1  0       // Digital Pin 2
#define intPin2  1       // Digital Pin 3

#define demoLight  13

// The larger the number, the quicker the joint will move
//   Try to keep number divisible to 180
#define incr1    5      // Shoulder Horizontal
#define incr2    5      // Shoulder Vertical
#define incr3    5      // Elbow
#define incr4    5      // Wrist Rotational
#define incr5    5      // Wrist Vertical

// Analog pins to read controller input
#define potpin1  0       // A0
#define potpin2  1       // A1
#define potpin3  2       // A2
#define potpin4  3       // A3
#define potpin5  4       // A4
#define potpin6  5       // A5

// Analog input pin that the Force Sensing Resistor (FSR) is attached
#define fsrPort  6       // A6

// Sensitivity for Force Sensing Resistor
#define FSRlimit 205


// grip positions
#define OPEN     105     // 114
#define CLOSE    140

#define variance 25    // reduces shaking

#define mid_scaled  90 // scaled value to put servo in middle


////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////


const int LCDdelay=10;  // conservative, 2 actually works




Servo myservo1;   // create servo object to control servo 1 
Servo myservo2;   // create servo object to control servo 2
Servo myservo3;   // create servo object to control servo 3
Servo myservo4;   // create servo object to control servo 4
Servo myservo5;   // create servo object to control servo 5
Servo myservo6;   // create servo object to control servo 6


int FSRvalue = 0; // value read from the FSR

int val1;         // variable to read the value from the first analog pin
int pos1 = 90;    // holds the possition of the first motor

int val2;         // variable to read the value from the second analog pin
int pos2 = 90;    // holds the possition of the second motor

int val3;         // variable to read the value from the third analog pin
int pos3 = 90;    // holds the possition of the third motor

int val4;         // variable to read the value from the fourth analog pin
int pos4 = 90;    // holds the possition of the fourth motor

int val5;         // variable to read the value from the fifthanalog pin
int pos5 = 90;    // holds the possition of the fifth motor

int val6;         // variable to read the value from the sixth analog pin
int pos6 = 0;     // holds the possition of the sixth motor


// Emergency Stop
//volatile boolean ESTOP = false;

// Flag for demo
volatile boolean DemoFlag = false;

////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////



/*
The control servo for the hand grip (myservo6) shakes quite a bit when in between open/close.
 It was decided to help eliminate this problem that we would just allow the grip to be either
 open or closed (no in between).  This is why it's output is hard set to either be OPEN or CLOSE.
 
 */
void setup() 
{
  attachInterrupt(intPin1, DemoSet, FALLING);
  //attachInterrupt(intPin2, StopNow, FALLING);

  // Pins 3,5,6,9,10, and 11 are chosen because they are PWM with analogWrite() function
  myservo1.attach(11);  // attaches the servo on pin 11 to the servo object
  myservo2.attach(10);  // attaches the servo on pin 10 to the servo object 
  myservo3.attach(9);   // attaches the servo on pin 9 to the servo object 
  myservo4.attach(6);   // attaches the servo on pin 6 to the servo object 
  myservo5.attach(5);   // attaches the servo on pin 5 to the servo object 
  myservo6.attach(3);   // attaches the servo on pin 3 to the servo object

  delay(1000);          // wait for 1 sec

  // set the initial possitions of the servos (put servos in middle)
  myservo1.write(mid_scaled);
  myservo2.write(mid_scaled);
  myservo3.write(mid_scaled);
  myservo4.write(mid_scaled);
  myservo5.write(mid_scaled);
  myservo6.write(OPEN);

  delay(1000);          // wait an additional second

  //ESTOP = false;

  LCDsetup();
} 

void loop() 
{ 
  
  if (DemoFlag)
  {    
    Demo();
    DemoFlag = false;
    
    // This eliminates the demo function being called twice due the the bouncing
    //   of the button.  This clears the execption flag that is set by the interrupt. 
    EIFR = 0x01;
    
    attachInterrupt(intPin1, DemoSet, FALLING);
  }
  
  //************************
  //** SHOULDER (HORIZONTAL) **
  //************************

  val1 = analogRead(potpin1);            // reads the value of the potentiometer (value between 0 and 1023) - 0->x3FF
  val1 = map(val1, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180)     - 0->xB4

  // move the servo one possition at a time
  if (val1 > (mid_scaled + variance))
  {
    pos1 += incr1; // pos1 = pos1 + incr1
    if (pos1 > 179) pos1 = 179;
    myservo1.write(pos1);                  // sets the servo position according to the scaled value 
  }
  else if (val1 < (mid_scaled - variance))
  {
    pos1 -= incr1;
    if (pos1 < 0) pos1 = 0;
    myservo1.write(pos1);                  // sets the servo position according to the scaled value 
  }

  //----------------------------------------------------------------------------------------------------------

  //**************************
  //** SHOULDER (VERTICAL) **
  //**************************

  val2 = analogRead(potpin2);            // reads the value of the potentiometer (value between 0 and 1023) 
  val2 = map(val2, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 

  // move the servo one possition at a time
  if (val2 > (mid_scaled + variance))
  {
    pos2 += incr2;
    if (pos2 > 179) pos2 = 179;
    myservo2.write(pos2);                  // sets the servo position according to the scaled value 
  }
  else if (val2 < (mid_scaled - variance))
  {
    pos2 -= incr2;
    if (pos2 < 0) pos2 = 0;
    myservo2.write(pos2);                  // sets the servo position according to the scaled value 
  }

  //----------------------------------------------------------------------------------------------------------

  //***********
  //** ELBOW **
  //***********

  val3 = analogRead(potpin3);            // reads the value of the potentiometer (value between 0 and 1023) 
  val3 = map(val3, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 

  // move the servo one possition at a time
  if (val3 > (mid_scaled + variance))
  {
    pos3 += incr3;
    if (pos3 > 179) pos3 = 179;
    myservo3.write(pos3);                  // sets the servo position according to the scaled value 
  }
  else if (val3 < (mid_scaled - variance))
  {
    pos3 -= incr3;
    if (pos3 < 0) pos3 = 0;
    myservo3.write(pos3);                  // sets the servo position according to the scaled value 
  }

  //----------------------------------------------------------------------------------------------------------

  //************************
  //** WRIST (ROTATIONAL) **
  //************************

  val4 = analogRead(potpin4);            // reads the value of the potentiometer (value between 0 and 1023) 
  val4 = map(val4, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 

  // move the servo one possition at a time
  if (val4 > (mid_scaled + variance))
  {
    pos4 += incr4;
    if (pos4 > 179) pos4 = 179;
    myservo4.write(pos4);                  // sets the servo position according to the scaled value 
  }
  else if (val4 < (mid_scaled - variance))
  {
    pos4 -= incr4;
    if (pos4 < 0) pos4 = 0;
    myservo4.write(pos4);                  // sets the servo position according to the scaled value 
  }

  //----------------------------------------------------------------------------------------------------------

  //**********************
  //** WRIST (VERTICAL) **
  //**********************

  val5 = analogRead(potpin5);            // reads the value of the potentiometer (value between 0 and 1023) 
  val5 = map(val5, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 

  // move the servo one possition at a time
  if (val5 > (mid_scaled + variance))
  {
    pos5 += incr5;
    if (pos5 > 179) pos5 = 179;
    myservo5.write(pos5);                  // sets the servo position according to the scaled value 
  }
  else if (val5 < (mid_scaled - variance))
  {
    pos5 -= incr5;
    if (pos5 < 0) pos5 = 0;
    myservo5.write(pos5);                  // sets the servo position according to the scaled value 
  }




  //----------------------------------------------------------------------------------------------------------

  FSRvalue = analogRead(fsrPort);
  FSRvalue = map(FSRvalue, 0, 1023, 0, 255);
  analogWrite(4, FSRvalue);

  //----------------------------------------------------------------------------------------------------------

  //*******************************
  //** HAND (GRIP = OPEN/CLOSED) **
  //*******************************

  val6 = analogRead(potpin6);            // reads the value of the potentiometer (value between 0 and 1023) 
  val6 = map(val6, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180)  

  // move the servo one possition at a time
  if (val6 > (mid_scaled + variance))
  {
    while( FSRvalue < FSRlimit && pos6 < CLOSE )
    {
      pos6++;
      myservo6.write(pos6);                  // sets the servo position according to the scaled value 
      
      FSRvalue = analogRead(fsrPort);
      FSRvalue = map(FSRvalue, 0, 1023, 0, 255);
      
      _delay_ms(10);
    }
    while ( ( CLOSE - pos6 < 5 ) && FSRvalue < 230 && pos6 < CLOSE )
    {
      pos6++;// = CLOSE;
      myservo6.write(pos6);
      
      FSRvalue = analogRead(fsrPort);
      FSRvalue = map(FSRvalue, 0, 1023, 0, 255);
      
      _delay_ms(10);
    }
    
    //Serial.print("                                            ");
    //Serial.println(pos6);
  }
  else if (val6 < (mid_scaled - variance))
  {
    pos6 = OPEN;
    myservo6.write(pos6);                  // sets the servo position according to the scaled value 
  }





  //----------------------------------------------------------------------------------------------------------


  LCDPrint();


  // This will keep all the possisions the same going up as going down.  By changing it from 179 to 180, the
  // same set of numbers will be used (at least at the current increment of 2).  Might need to be changed later
  // down the road
  /*
  if (pos1 == 179) pos1 = 180;
  if (pos2 == 179) pos2 = 180;
  if (pos3 == 179) pos3 = 180;
  if (pos4 == 179) pos4 = 180;
  if (pos5 == 179) pos5 = 180;
  */

  delay(20);                           // waits for the servo to get there 

} 



// This sets the flag forthe automated demonstration that the arm will perform for the users
void DemoSet()
{
  // Only Change if the demo is not already running
  if (!DemoFlag)
  {
    // Make sure that the interrupt doesn't keep happening until the demo is over
    detachInterrupt(intPin1);
  
    // Signal to the loop() function that a demonstration has been requested.
    DemoFlag = true;

    // This isn't needed here, but put it here as well for a double check.
    // Reason same as mentioned above for the EIFR
    EIFR = 0x01;
  }

}


void Demo()
{
  pinMode(12,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(7,OUTPUT);
  
  pinMode(demoLight,OUTPUT);
  digitalWrite(demoLight,HIGH);

  // Set initial possitions
  int p1 = pos1;
  int p2 = pos2;
  int p3 = pos3;
  int p4 = pos4;
  int p5 = pos5;
  int p6 = OPEN;

  // Middle locations (location of object to pick up)
  int mloc1 = 55;
  int mloc2 = 80;
  int mloc3 = 133;
  int mloc4 = 179;
  int mloc5 = 0;
  int mloc6 = CLOSE;

  // End location to where to move the object.
  int eloc1 = 30;
  int eloc2 = 75;
  int eloc3 = 120;
  int eloc4 = 165;
  int eloc5 = 135;
  int eloc6 = OPEN;
  
  // Variable to signal that all servos have arrived at their
  //   final locations and to end the current loop
  int breakLoop = B00000000;

  // LCD housekeeping
  clearLCD();
  cursorHome();
  Serial.println();
  Serial.println("             Starting  Demo            "); //14
  Serial.println("              Please  Wait             "); //12
  
  _delay_ms(500);
  
  // go to middle locations
  for (int i = 0; i < 179 && breakLoop != 0x1F; i++)
  {
    if ( p1 < mid_scaled )
    {
      p1 += incr1;
      if (p1 >= mid_scaled)
      {
        p1 = mid_scaled;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }
    else if ( p1 > mid_scaled )
    {
      p1 -= incr1;
      if (p1 <= mid_scaled)
      {
        p1 = mid_scaled;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }

    if ( p2 < mid_scaled )
    {
      p2 += incr2;
      if (p2 >= mid_scaled)
      {
        p2 = mid_scaled;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }
    else if ( p2 > mid_scaled )
    {
      p2 -= incr2;
      if (p2 <= mid_scaled)
      {
        p2 = mid_scaled;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }

    if ( p3 < mid_scaled )
    {
      p3 += incr3;
      if (p3 >= mid_scaled)
      { 
        p3 = mid_scaled;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }
    else if ( p3 > mid_scaled )
    {  
      p3 -= incr3;
      if (p3 <= mid_scaled)
      { 
        p3 = mid_scaled;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }

    if ( p4 < mid_scaled )
    {
      p4 += incr4;
      if (p4 >= mid_scaled)
      { 
        p4 = mid_scaled;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }
    else if ( p4 > mid_scaled )
    {
      p4 -= incr4;
      if (p4 <= mid_scaled)
      { 
        p4 = mid_scaled;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }

    if ( p5 < mid_scaled )
    {
      p5 += incr5;
      if (p5 >= mid_scaled)
      { 
        p5 = mid_scaled;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }
    else if ( p5 > mid_scaled )
    {
      p5 -= incr5;
      if (p5 <= mid_scaled)
      { 
        p5 = mid_scaled;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }
    
    _delay_ms(25);
  }
  myservo6.write(p6);
  digitalWrite(12,HIGH);
  
  
  // delay 2 seconds
  _delay_ms(2000);

  breakLoop = 0x00;
  // go to object location
  for (int i = 0; i < 179 && breakLoop != 0x1F; i++)
  {
    if ( p1 < mloc1 )
    {
      p1 += incr1;
      if (p1 >= mloc1)
      {
        p1 = mloc1;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }
    else if ( p1 > mloc1 )
    {
      p1 -= incr1;
      if (p1 <= mloc1)
      {
        p1 = mloc1;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }

    if ( p2 < mloc2 )
    {
      p2 += incr2;
      if (p2 >= mloc2)
      {
        p2 = mloc2;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }
    else if ( p2 > mloc2 )
    {
      p2 -= incr2;
      if (p2 <= mloc2)
      {
        p2 = mloc2;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }

    if ( p3 < mloc3 )
    {
      p3 += incr3;
      if (p3 >= mloc3)
      { 
        p3 = mloc3;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }
    else if ( p3 > mloc3 )
    {  
      p3 -= incr3;
      if (p3 <= mloc3)
      { 
        p3 = mloc3;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }

    if ( p4 < mloc4 )
    {
      p4 += incr4;
      if (p4 >= mloc4)
      { 
        p4 = mloc4;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }
    else if ( p4 > mloc4 )
    {
      p4 -= incr4;
      if (p4 <= mloc4)
      { 
        p4 = mloc4;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }

    if ( p5 < mloc5 )
    {
      p5 += incr5;
      if (p5 >= mloc5)
      { 
        p5 = mloc5;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }
    else if ( p5 > mloc5 )
    {
      p5 -= incr5;
      if (p5 <= mloc5)
      { 
        p5 = mloc5;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }




    cursorHome();

    Serial.print(" V Shoulder  ");
    Serial.print(" H Shoulder  ");
    Serial.print("    Elbow    ");

    if (p1 >= 100) Serial.print("     ");
    else if (p1 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p1);
    Serial.print("     ");

    if (p2 >= 100) Serial.print("     ");
    else if (p2 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p2);
    Serial.print("     ");

    if (p3 >= 100) Serial.print("     ");
    else if (p3 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p3);
    Serial.print("    ");

    newLine();

    Serial.print("  Wrist Rot  ");
    Serial.print(" Wrist  Flex ");
    Serial.print("    Hand     ");

    if (p4 >= 100) Serial.print("     ");
    else if (p4 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p4);
    Serial.print("     ");

    if (p5 >= 100) Serial.print("     ");
    else if (p5 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p5);
    Serial.print("     ");

    if (p6 == OPEN) Serial.println("     open  ");
    else Serial.println("     closed");
    Serial.print("  ");

  }

  // delay 2 seconds
  _delay_ms(2000);


  while ( FSRvalue < FSRlimit && p6 < mloc6 )
  {
    p6++;
    myservo6.write(p6);
    
    FSRvalue = analogRead(fsrPort);
    FSRvalue = map(FSRvalue, 0, 1023, 0, 255);
    
    _delay_ms(10);
  }
  
  
  if (p6 == OPEN) Serial.println("     open  ");
  else Serial.println("     closed");
  Serial.print("  ");
  digitalWrite(8,HIGH);


  // delay 2 seconds
  _delay_ms(2000);

  breakLoop = 0x00;
  // time to move to end destination
  for (int i = 0; i < 179 && breakLoop != 0x1F; i++)
  {
    if ( p1 < eloc1 )
    {
      p1 += incr1;
      if (p1 >= eloc1)
      {
        p1 = eloc1;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }
    else if ( p1 > eloc1 )
    {
      p1 -= incr1;
      if (p1 <= eloc1)
      {
        p1 = eloc1;
        breakLoop |= 0x10;
      }
      myservo1.write(p1);
    }

    if ( p2 < eloc2 )
    {
      p2 += incr2;
      if (p2 >= eloc2)
      {
        p2 = eloc2;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }
    else if ( p2 > eloc2 )
    {
      p2 -= incr2;
      if (p2 <= eloc2)
      {
        p2 = eloc2;
        breakLoop |= 0x08;
      }
      myservo2.write(p2);
    }

    if ( p3 < eloc3 )
    {
      p3 += incr3;
      if (p3 >= eloc3)
      { 
        p3 = eloc3;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }
    else if ( p3 > eloc3 )
    {  
      p3 -= incr3;
      if (p3 <= eloc3)
      { 
        p3 = eloc3;
        breakLoop |= 0x04;
      }
      myservo3.write(p3);
    }

    if ( p4 < eloc4 )
    {
      p4 += incr4;
      if (p4 >= eloc4)
      { 
        p4 = eloc4;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }
    else if ( p4 > eloc4 )
    {
      p4 -= incr4;
      if (p4 <= eloc4)
      { 
        p4 = eloc4;
        breakLoop |= 0x02;
      }
      myservo4.write(p4);
    }

    if ( p5 < eloc5 )
    {
      p5 += incr5;
      if (p5 >= eloc5)
      { 
        p5 = eloc5;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }
    else if ( p5 > eloc5 )
    {
      p5 -= incr5;
      if (p5 <= eloc5)
      { 
        p5 = eloc5;
        breakLoop |= 0x01;
      }
      myservo5.write(p5);
    }


    cursorHome();

    Serial.print(" V Shoulder  ");
    Serial.print(" H Shoulder  ");
    Serial.print("    Elbow    ");

    if (p1 >= 100) Serial.print("     ");
    else if (p1 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p1);
    Serial.print("     ");

    if (p2 >= 100) Serial.print("     ");
    else if (p2 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p2);
    Serial.print("     ");

    if (p3 >= 100) Serial.print("     ");
    else if (p3 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p3);
    Serial.print("    ");

    newLine();

    Serial.print("  Wrist Rot  ");
    Serial.print(" Wrist  Flex ");
    Serial.print("    Hand     ");

    if (p4 >= 100) Serial.print("     ");
    else if (p4 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p4);
    Serial.print("     ");

    if (p5 >= 100) Serial.print("     ");
    else if (p5 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p5);
    Serial.print("     ");
  }

  // delay 2 seconds
  _delay_ms(2000);


  while ( p6 > eloc6 )
  {
    p6--;
    myservo6.write(p6);
  }
  if (p6 == OPEN) Serial.println("     open  ");
  else Serial.println("     closed");
  Serial.print("  ");
  digitalWrite(7,HIGH);


  // delay 3 seconds
  _delay_ms(3000);


  // move back to original location before demo
  for (int i = 0; i < 179; i++)
  {
    if ( p1 < pos1 )
    {
      p1++;
      myservo1.write(p1);
    }
    else if ( p1 > pos1 )
    {
      p1--;
      myservo1.write(p1);
    }

    if ( p2 < pos2 )
    {
      p2++;
      myservo2.write(p2);
    }
    else if ( p2 > pos2 )
    {
      p2--;
      myservo2.write(p2);
    }

    if ( p3 < pos3 )
    {
      p3++;
      myservo3.write(p3);
    }
    else if ( p3 > pos3 )
    {  
      p3--;
      myservo3.write(p3);
    }

    if ( p4 < pos4 )
    {
      p4++;
      myservo4.write(p4);
    }
    else if ( p4 > pos4 )
    {
      p4--;
      myservo4.write(p4);
    }

    if ( p5 < pos5 )
    {
      p5++;
      myservo5.write(p5);
    }
    else if ( p5 > pos5 )
    {
      p5--;
      myservo5.write(p5);
    }


    cursorHome();

    Serial.print(" V Shoulder  ");
    Serial.print(" H Shoulder  ");
    Serial.print("    Elbow    ");

    if (p1 >= 100) Serial.print("     ");
    else if (p1 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p1);
    Serial.print("     ");

    if (p2 >= 100) Serial.print("     ");
    else if (p2 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p2);
    Serial.print("     ");

    if (p3 >= 100) Serial.print("     ");
    else if (p3 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p3);
    Serial.print("    ");

    newLine();

    Serial.print("  Wrist Rot  ");
    Serial.print(" Wrist  Flex ");
    Serial.print("    Hand     ");

    if (p4 >= 100) Serial.print("     ");
    else if (p4 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p4);
    Serial.print("     ");

    if (p5 >= 100) Serial.print("     ");
    else if (p5 >= 10) Serial.print("      ");
    else Serial.print("       ");
    Serial.print(p5);
    Serial.print("     ");
  }

  // open/close hand to what it was before
  myservo6.write(pos6);
  if (pos6 == OPEN) Serial.println("     open  ");
  else Serial.println("     closed");
  Serial.print("  ");

  digitalWrite(12,LOW);
  digitalWrite(8,LOW);
  digitalWrite(7,LOW);
  digitalWrite(demoLight,LOW);
  
}


/*
void StopNow()
{
  ESTOP = true;
  detachInterrupt(intPin1);
  detachInterrupt(intPin2);
}
*/


void LCDsetup()
{
  delay(3000); // allows 3 additional seconds for the welcome screen to display

  Serial.begin(baudRate);
  // setStartScreen();  // ONLY UNCOMMENT IF THE LCD SCREEN NEEDS REPROGRAMING
  clearLCD();

  // Display a welcome message
  newLine();


  delay(1000);
  clearLCD();
}



/*
(9/18/11)
 The LCD display will be a good addition for a couple of reasons:
 1. It will be useful for ourselves when debugging the code
 2. Determining appropriate values when coding the demo modes
 3. Give the students a numerical value to the angle they are moving
 their part of the arm.
 4. Allow the teacher to assist in directing students movements
 5. Give the teacher yet another teaching tool.  Could help to teach
 torch and force vectors
 (in no specific order)
 
 (9/22/11)
 Thought about makig a function to print the individual data:  ie. printData(servo_num, value)
 Where 'servo_num' being the servo being controlled and 'value' being the degree of movement.
 Decided against this because when dealing with hardware, when passing arguments to functions,
 it pushed the values onto the stack then pops them off when using them.  This takes quite
 a bit more clock cycles and is more memory intensive.  With a small micro like this, it could
 potentially be too costly in terms of memory and speed.
 NOTE: calling functions with no parameters is alright as it does not push and pop values
 */
void LCDPrint()
{
  //setBaud();
  cursorHome();

  Serial.print(" V Shoulder  "); // Servo 1 - V Shoulder
  Serial.print(" H Shoulder  "); // Servo 2 - H Shoulder
  Serial.print("    Elbow    "); // Servo 3 - Elbow

  if (pos1 >= 100) Serial.print("     ");
  else if (pos1 >= 10) Serial.print("      ");
  else Serial.print("       ");
  Serial.print(pos1);
  Serial.print("     ");

  if (pos2 >= 100) Serial.print("     ");
  else if (pos2 >= 10) Serial.print("      ");
  else Serial.print("       ");
  Serial.print(pos2);
  Serial.print("     ");

  if (pos3 >= 100) Serial.print("     ");
  else if (pos3 >= 10) Serial.print("      ");
  else Serial.print("       ");
  Serial.print(pos3);
  Serial.print("    ");

  newLine();

  Serial.print("  Wrist Rot  "); // Servo 4 - Wrist Rot
  Serial.print(" Wrist  Flex "); // Servo 5 - Wrist Flex
  Serial.print("    Hand     "); // Servo 6 - Hand

  if (pos4 >= 100) Serial.print("     ");
  else if (pos4 >= 10) Serial.print("      ");
  else Serial.print("       ");
  Serial.print(pos4);
  Serial.print("     ");

  if (pos5 >= 100) Serial.print("     ");
  else if (pos5 >= 10) Serial.print("      ");
  else Serial.print("       ");
  Serial.print(pos5);
  Serial.print("     ");


  if (pos6 == OPEN) Serial.println("     open  ");
  else Serial.println("     closed");
  Serial.print("  ");


  //Serial.println("");
  //Serial.println("");
  //Serial.println(FSRvalue);
  //Serial.println("");
  //Serial.println("");
}


//-----------------------------------------
//--------  LCD Special Functions  --------
//-----------------------------------------

// clear the LCD
void clearLCD()
{
  Serial.print(12, BYTE);  
  delay(LCDdelay);
}

// start a new line
void newLine() 
{ 
  Serial.print(10, BYTE); // new line
  Serial.print(13, BYTE); // carriage return
}

// move the cursor to the home position
void cursorHome()
{
  Serial.print(254, BYTE);
  Serial.print(72, BYTE);
}

// move the cursor to a specific place
// e.g.: cursorSet(3,2) sets the cursor to x = 3 and y = 2
void cursorSet(int xpos, int ypos)
{  
  Serial.print(254, BYTE);
  Serial.print(71, BYTE);               
  Serial.print(xpos);   //Column position   
  Serial.print(ypos); //Row position 
} 

// backspace and erase previous character
void backSpace() 
{ 
  Serial.print(8, BYTE); 
}

// move cursor left
void cursorLeft()
{    
  Serial.print(254, BYTE); 
  Serial.print(76, BYTE);   
}

// move cursor right
void cursorRight()
{
  Serial.print(254, BYTE); 
  Serial.print(77, BYTE);   
}

void setBaud()
{
  Serial.print(254, BYTE);
  Serial.print(57, BYTE);
  //Serial.print(103, BYTE);  // 9600
  Serial.print(51, BYTE);  //19200
}

void setStartScreen()
{
  cursorHome();
  Serial.print(254, BYTE);
  Serial.print(64, BYTE);
  Serial.print("              Robotic  Arm              ");
  Serial.print("           Purdue  University           ");
  Serial.print(" Greater Lafayette Elementary Education ");
  Serial.print("                                        ");

  // Visual display of when it has updated the data
  pinMode(13,OUTPUT);
  digitalWrite(13,HIGH);
  delay(5000);
  digitalWrite(13,LOW);
  delay(5000);
}





